
import * as THREE from 'three';
import * as CANNON from 'cannon-es';
import CannonDebugger from 'cannon-es-debugger';
import SceneInit from "./SceneInit.js";

const test = new SceneInit('canvas');
test.initialize();
test.animate();

const axesHelper = new THREE.AxesHelper(8);
test.scene.add(axesHelper);

// ============
// part 1
// set up world physics with gravity
// ============
const physicsWorld = new CANNON.World({
    gravity: new CANNON.Vec3(0, -9.82, 0),
});

// Sweep and prune broadphase
physicsWorld.broadphase = new CANNON.SAPBroadphase(physicsWorld)

// Disable friction by default
physicsWorld.defaultContactMaterial.friction = 0.2;

// Build the car chassis
const chassisShape = new CANNON.Box(new CANNON.Vec3(2, 0.5, 1))
const chassisBody = new CANNON.Body({ mass: 150 })
chassisBody.addShape(chassisShape)
chassisBody.position.set(0, 4, 0)
chassisBody.angularVelocity.set(0, 0.5, 0)

// demo.addVisual(chassisBody)
const boxGeometry = new THREE.BoxGeometry(4, 1, 2);
const boxMaterial = new THREE.MeshNormalMaterial();
const boxMesh = new THREE.Mesh(boxGeometry, boxMaterial);
test.scene.add(boxMesh);


// Create the vehicle
const vehicle = new CANNON.RaycastVehicle({
    chassisBody,
})

const wheelOptions = {
    radius: 0.5,
    directionLocal: new CANNON.Vec3(0, -1, 0),
    suspensionStiffness: 30,
    suspensionRestLength: 0.3,
    frictionSlip: 1.4,
    dampingRelaxation: 2.3,
    dampingCompression: 4.4,
    maxSuspensionForce: 100000,
    rollInfluence: 0.01,
    axleLocal: new CANNON.Vec3(0, 0, 1),
    chassisConnectionPointLocal: new CANNON.Vec3(-1, 0, 1),
    maxSuspensionTravel: 0.3,
    customSlidingRotationalSpeed: -30,
    useCustomSlidingRotationalSpeed: true,
}

wheelOptions.chassisConnectionPointLocal.set(-1, 0, 1)
vehicle.addWheel(wheelOptions)

wheelOptions.chassisConnectionPointLocal.set(-1, 0, -1)
vehicle.addWheel(wheelOptions)

wheelOptions.chassisConnectionPointLocal.set(1, 0, 1)
vehicle.addWheel(wheelOptions)

wheelOptions.chassisConnectionPointLocal.set(1, 0, -1)
vehicle.addWheel(wheelOptions)

vehicle.addToWorld(physicsWorld)

const wheels:any[] = [];

// Add the wheel bodies
const wheelBodies: any[] = []
const wheelMaterial = new CANNON.Material('wheel')
vehicle.wheelInfos.forEach((wheel) => {
    const cylinderShape = new CANNON.Cylinder(wheel.radius, wheel.radius, wheel.radius / 2, 20)
    const wheelBody = new CANNON.Body({
        mass: 0,
        material: wheelMaterial,
    })
    wheelBody.type = CANNON.Body.KINEMATIC
    wheelBody.collisionFilterGroup = 0 // turn off collisions
    const quaternion = new CANNON.Quaternion().setFromEuler(-Math.PI / 2, 0, 0)
    wheelBody.addShape(cylinderShape, new CANNON.Vec3(), quaternion)
    wheelBodies.push(wheelBody)
    // demo.addVisual(wheelBody)
    physicsWorld.addBody(wheelBody);

    let mesh = getWheelMesh(wheel.radius);
    test.scene.add(mesh);
    mesh.userData.body = wheelBody;
    wheels.push(mesh);

})

function getWheelMesh(radius: number) {
    var obj = new THREE.Object3D();
    var geo = new THREE.CylinderGeometry(radius, radius, radius / 2, 20);
    var mat = new THREE.MeshLambertMaterial({ color: 0xff3333 });
    var mesh = new THREE.Mesh(geo, mat);
    obj.add(mesh);
    // var w = mesh.clone();
    mesh.rotateOnAxis(new THREE.Vector3(1, 0, 0), Math.PI / 2);
    // obj.add(mesh);
    return obj;
}

// Update the wheel bodies
physicsWorld.addEventListener('postStep', () => {
    for (let i = 0; i < vehicle.wheelInfos.length; i++) {
        vehicle.updateWheelTransform(i)
        const transform = vehicle.wheelInfos[i].worldTransform
        const wheelBody = wheelBodies[i]
        wheelBody.position.copy(transform.position)
        wheelBody.quaternion.copy(transform.quaternion)
    }
})

// Add the ground
const sizeX = 64
const sizeZ = 64
const matrix: any[] = []
for (let i = 0; i < sizeX; i++) {
    matrix.push([])
    for (let j = 0; j < sizeZ; j++) {
        if (i === 0 || i === sizeX - 1 || j === 0 || j === sizeZ - 1) {
            const height = 3
            matrix[i].push(height)
            continue
        }

        const height = Math.cos((i / sizeX) * Math.PI * 5) * Math.cos((j / sizeZ) * Math.PI * 5) * 2 + 2
        matrix[i].push(height)
    }
}

const groundMaterial = new CANNON.Material('ground')
const heightfieldShape = new CANNON.Heightfield(matrix, {
    elementSize: 100 / sizeX,
})
const heightfieldBody = new CANNON.Body({ mass: 0, material: groundMaterial })
heightfieldBody.addShape(heightfieldShape)
heightfieldBody.position.set(
    // -((sizeX - 1) * heightfieldShape.elementSize) / 2,
    -(sizeX * heightfieldShape.elementSize) / 2,
    -1,
    // ((sizeZ - 1) * heightfieldShape.elementSize) / 2
    (sizeZ * heightfieldShape.elementSize) / 2
)
heightfieldBody.quaternion.setFromEuler(-Math.PI / 2, 0, 0)
physicsWorld.addBody(heightfieldBody)
// demo.addVisual(heightfieldBody)

// Define interactions between wheels and ground
const wheel_ground = new CANNON.ContactMaterial(wheelMaterial, groundMaterial, {
    friction: 0.3,
    restitution: 0,
    contactEquationStiffness: 1000,
})
physicsWorld.addContactMaterial(wheel_ground)

const cannonDebugger = new CannonDebugger(test.scene, physicsWorld);

// Keybindings
// Add force on keydown
document.addEventListener('keydown', (event) => {
    const maxSteerVal = 0.5
    const maxForce = 1000
    const brakeForce = 1000000

    switch (event.key) {
        case 'w':
        case 'ArrowUp':
            vehicle.applyEngineForce(-maxForce, 2)
            vehicle.applyEngineForce(-maxForce, 3)
            break

        case 's':
        case 'ArrowDown':
            vehicle.applyEngineForce(maxForce, 2)
            vehicle.applyEngineForce(maxForce, 3)
            break

        case 'a':
        case 'ArrowLeft':
            vehicle.setSteeringValue(maxSteerVal, 0)
            vehicle.setSteeringValue(maxSteerVal, 1)
            break

        case 'd':
        case 'ArrowRight':
            vehicle.setSteeringValue(-maxSteerVal, 0)
            vehicle.setSteeringValue(-maxSteerVal, 1)
            break

        case 'b':
            vehicle.setBrake(brakeForce, 0)
            vehicle.setBrake(brakeForce, 1)
            vehicle.setBrake(brakeForce, 2)
            vehicle.setBrake(brakeForce, 3)
            break
    }
})

// Reset force on keyup
document.addEventListener('keyup', (event) => {
    switch (event.key) {
        case 'w':
        case 'ArrowUp':
            vehicle.applyEngineForce(0, 2)
            vehicle.applyEngineForce(0, 3)
            break

        case 's':
        case 'ArrowDown':
            vehicle.applyEngineForce(0, 2)
            vehicle.applyEngineForce(0, 3)
            break

        case 'a':
        case 'ArrowLeft':
            vehicle.setSteeringValue(0, 0)
            vehicle.setSteeringValue(0, 1)
            break

        case 'd':
        case 'ArrowRight':
            vehicle.setSteeringValue(0, 0)
            vehicle.setSteeringValue(0, 1)
            break

        case 'b':
            vehicle.setBrake(0, 0)
            vehicle.setBrake(0, 1)
            vehicle.setBrake(0, 2)
            vehicle.setBrake(0, 3)
            break
    }
})

// demo.start()

const animate = () => {
    physicsWorld.fixedStep();
    cannonDebugger.update();
    boxMesh.position.copy(chassisBody.position);
    boxMesh.quaternion.copy(chassisBody.quaternion);

    wheels.forEach(w => {
        w.position.copy(w.userData.body.position);
        w.quaternion.copy(w.userData.body.quaternion);
    })
    // sphereMesh1.position.copy(wheelBody1.position);
    // sphereMesh1.quaternion.copy(wheelBody1.quaternion);
    // sphereMesh2.position.copy(wheelBody2.position);
    // sphereMesh2.quaternion.copy(wheelBody2.quaternion);
    // sphereMesh3.position.copy(wheelBody3.position);
    // sphereMesh3.quaternion.copy(wheelBody3.quaternion);
    // sphereMesh4.position.copy(wheelBody4.position);
    // sphereMesh4.quaternion.copy(wheelBody4.quaternion);
    window.requestAnimationFrame(animate);
};
animate();